#include<iostream>
#include<cstdlib>
#include<ctime>
using namespace std;
#define N 6
#define L 1
#define V0 1
#define speed 0.2
#define Steps 20

void cpu_simple_update(double* hvx, int n)
{
	double* tmp_hvx = new double [n];
	memcpy(tmp_hvx, hvx, sizeof(double)*n);
	double avg_hvx = 0;
	for (int j = 0; j < n; j++)
	{
		avg_hvx += hvx[j];
	}
	avg_hvx /= n;
	for (int i = 0; i < n; i++)
		tmp_hvx[i] += speed*(avg_hvx - hvx[i]);
	memcpy(hvx, tmp_hvx, sizeof(double)*N);
	delete [] tmp_hvx;
}

void cpu_square_distance(double* hx, int n, double x, 
						double* dis)
{
	for (int i = 0; i < n; i++)
		dis[i] = (hx[i] - x)*(hx[i] - x);
}

void cpu_k_update_single(double* hx, double* hvx, int n, int k, double x, double vx,
				  double *new_vx, double Speed)
{
	double* dis = new double [n];
	cpu_square_distance(hx, n, x, dis);
	int* index = new int [n];
	findKthMin2(hx, n, k, index);
	double avg_vx = 0;
	for (int j = 0; j <k; j++)
		avg_vx += hvx[index[j]];
	avg_vx /= k;
	*new_vx = vx + Speed*(avg_vx - vx);
	delete [] dis;
	delete [] index;
}

void cpu_k_update_all(double* hx, double* hvx, int n, int k, double Speed)
{
	double* tmp_hx = new double [n];
	double* tmp_hvx = new double [n];
	memcpy(tmp_hx, hx, sizeof(double)*n);
	memcpy(tmp_hvx, hvx, sizeof(double)*n);
	for (int i = 0; i < n; i++)
	{
		double new_vx;
		cpu_k_update_single(hx, hvx, n,k,hx[i],hvx[i],&new_vx,Speed);
		tmp_hvx[i] = new_vx;
	}
	memcpy(hx, tmp_hx, sizeof(double)*n);
	memcpy(hvx, tmp_hvx, sizeof(double)*n);
	delete [] tmp_hx;
	delete [] tmp_hvx;
}

void cpu_display( double* hvx, int n)
{
	for (int i = 0; i < n; i++)
		cout<<hvx[i]<<" ";
	cout<<endl;
}

int main()
{
	srand(0);
	double* hx = new double [N];
	double* hvx = new double [N];
	for(int i = 0; i < N; i++)
	{
		hx[i] = (double)rand() / RAND_MAX * L;
		hvx[i] = (double)rand() / RAND_MAX * V0;
	}

	for (int i = 0; i < Steps; i++)
	{
		cpu_simple_update(hvx, N);
		cpu_display(hvx, N);
		double s = 0;
		for(int j = 0; j < N; j++)
			s += hvx[j];
		cout<<s<<endl;	
	}
	system("pause");
	return 0;
}
